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I. INTRODUCTION 

A.   GENERAL 

Since  the  early  1960s  the  U.S  Navy  has  been  interested  in 
Unmanned  Underwater  Vehicles (UUV) ,  which  include  Autonomous 
Underwater  Vehicles  (AUV)  and  Remotely  Operated  Vehicles  (ROV)  . 
The  latter  are  tethered  to  a  supporting  platform,  where  an 
operator  provides  necessary  control  signals.  In  this  case  most 
of  the  mission  planning  and  on-line  decision  making  is 
accomplished  by  the  operator.  Increasing  computer  capabilities 
have  decreased  the  need  to  tether  the  vehicle  to  prove 
guidance  and  navigational  support.  The  improvements  allowed 
underwater  vehicles  to  be  autonomous.  [Ref.  1] 

Autonomous  vehicles  can  go  where  humans  cannot  go  and 
do  not  want  to  go.  Autonomous  vehicles  are  capable  of 
receiving  initial  input,  moving  to  another  location,  executing 
a  mission,  and  returning  with  the  requested  results  and  the 
data.  In  addition  to  performing  labor  intensive  and  repetitive 
tasks,  these  vehicles  can  perform  their  tasks  faster  and  with 
greater  precision  than  humans,  and  can  also  proceed  into 
hostile  or  contaminated  environments.  [Ref.  2] 

At  the  Naval  Postgraduate  School  the  interest  in 
autonomous  underwater  vehicles  has  manifested  itself   in 


current  research  which  supports  development  of  a  prototype 
vehicle  named  NPS  AUVII. 

B.  GOAL 

If  an  autonomous  underwater  vehicle  is  to  be  completely 
autonomous,  it  must  have  a  navigation  system  capable  of 
estimating  its  own  position  at  all  times.  The  effort  in  this 
thesis  is  mainly  concerned  with  the  navigation  problem  of  the 
AUV.  Compared  to  autonomous  land  vehicles,  AUVs  have  many 
disadvantages,  such  as  the  presence  of  highly  nonlinear 
vehicle  dynamics  that  are  often  uncertain  and  the  presence  of 
currents  whose  effects  cannot  be  detected  by  inertial 
navigation  systems.  One  possible  solution  to  this  problem  is 
the  use  of  velocity  measurements  relative  to  the  ocean  floor, 
using,  for  example,  a  doppler  sonar.  However,  the  cost,  size 
and  the  depth  of  the  ocean  often  prevents  use  of  doppler 
sonar  on  standard  AUV's.  Visual  information  (sonar  and 
optical)  seems  a  viable  approach  in  environments  having 
landmarks  as  references,  such  as  buoys,  piers,  rocks,  and  man 
made  structures. 

C.  METHOD  OF  APPROACH 

This  thesis  develops  a  short  range  navigation  algorithm 
for  the  planar  motion  of  the  NPS  AUVII  vehicle.  The  system 
uses  sonar  returns  and  incorporates  them  with  a  potential 
function  which  defines  the  map  of  the  operation  area.  The 


measurements  obtained  from  sonar  are  filtered  through  a  Kalman 
estimator  using  extended  Kalman  filtering.  The  main  feature  of 
the  algorithm  is  the  use  of  a  potential  function  to  define  the 
area  of  operation.  By  use  of  this  potential  function,  the 
system  discriminates  the  objects  which  are  not  on  the  map  , 
and  yields  a  robust  and  reliable  solution  to  the  short  range 
navigation  problem  of  the  AUV.  The  algorithm  is  first  applied 
in  a  rectangular  shaped  pool  and  later  applied  to  an 
environment  without  any  borders  (e.g  open  sea) . 

This  thesis  consists  of  five  parts.  Chapter  II  discusses 
the  theory  behind  the  navigator  design  and  the  modeling 
assumptions  in  this  study.  Chapter  III  discusses  the  details 
of  the  potential  function  approach  in  the  implementation  of  a 
navigation  algorithm  together  with  simulation  results.  The 
results  using  the  actual  data  collected  in  the  water  tank 
are  also  discussed.  Chapter  IV  discusses  the  implementation  of 
neural  networks  for  the  navigation  of  AUV.  Consequently 
Chapter  V  summarizes  the  results  and  conclusions  of  the  study, 
and  provides  recommendations  for  further  investigation. 


II.    BACKGROUND 

A.   KALMAN  FILTERING 

Since  it  was  derived  by  R.E.  Kalman  in  I960,  the  Kalman 
filter  has  been  used  extensively  in  the  design  of  optimal 
estimation  algorithms.  Its  rapid  acceptance  and  subsequent 
success  are  due  to  the  Kalman  Filter's  recursive  nature  and 
optimality . 

The  Kalman  filter  estimates  the  states  of  a  system  given 
a  set  of  inputs  to  the  system  and  a  set  of  measurements  both 
corrupted  by  noise  [Ref .  3]  .  The  system  is  assumed  to  be 
driven  by  both  a  known  input  and  a  random  input : 

x{k+l)  =Qx{k)  +Alu{k)  +A2w(k)  ,  (2.1) 

where  u(k)  is  the  deterministic  input,  w(k)  is  plant  driving 
noise,  <£  is  the  state  transition  matrix  and  x(k)  is  the  state 
vector.  We  also  assume  w(k)  is  a  zero  mean  gaussian  random 
vector  with  covariance: 

E[w{k)  w(k+n)  ]  =Q6  (n)  ,  (2.2) 

where  Q  is  the  covariance  matrix  of  plant  noise  and  6  (n)  is 
impulse  function.  The  measurement  process  can  be  modeled  as: 


y(k)=Hx(k)+v(k)  ,  (2.3) 

where  v(k)  is  zero  mean  additive  gaussian  noise  and  H  is  the 
measurement  matrix.  Measurement  noise  v(k)  has  covariance: 

E[v{k)  v(k+n) ]  =R6  (n) ,  (2.4) 

where  R  is  the  measurement  error  covariance  matrix. 

Using  the  orthogonality  principle  it  can  be  shown  that  the 
recursive  solution  to  this  problem  has  the  following  form: 

£(ic+l|/c+l)  =*(ic+l|/c)  +G(ic+l)  [y(k+l)  -y(k+l\k)]  .         (2.5) 

Given  the  current  estimate  we  can  predict  ahead  by  simply 
using  the  state  equation: 

J?(Jc+l|Jt)=^(Jt|Jt)+A1u(ic)l  (2.6) 

which  yields  the  optimal  predictor.  We  can  reasonably  estimate 
y(k+l)  by  simply  using   the  H  matrix  such  that: 

y(k+l\k)  =Hx(k+l\k)  .  (2.7) 

The  Kalman  gain  G  depends  on  the  covariance  matrix  of 
estimation  error  which  is  defined  as: 

P(k\k)    =  E[{x{k)  -$(k\k)  }  [x(k)  -£(k\k)  }T]  .  (2.8) 


The  Kalman  gains  G(k+1)  are  found  by  applying  the 
orthogonality  principle  as  shown  in  [Ref.  3],  which  leads  to 
the  following  recursive  equations: 

G{k+1)    =  P(k+l\k)HT[HPHT   +  R]'1  (2.9) 

P(k+l\k+l)  =[I  -G(k+1)H]  P{k+l\k)  (2.10) 

P(k+l\k)    =  4>j£(jc|£)4>t  +  A2£)A2r  (2.11) 

The  filter  equations  can  be  summed  up  as  follows: 

&{k+l\k)    =  $2{k\k)    +  Axu(k)  (2.12) 

?(k+i\k)    =  HX(k+i\k)  (2.13) 

X(k+i\k+i)  =&{k+l\k)  +G(k+l)  [y(k+D  -?{k+l\k)  ]       (2.14) 

As  can  be  seen  from  the  above  equations,  for  a  Linear  Time 
Invariant  (LTD  system,  the  Kalman  gains  can  be  computed  off- 
line and  stored  in  the  computer  and  used  as  look-up  tables.  In 
order  to  start  the  filter  algorithm,  a  priori  values  of  the 
state  estimates  and  the  error  covariance  are  needed.  The 
proper  choice  for  these  estimates  are: 

*(0|0)  =  E[x(0)] ,  (2.15) 

P(0|0)  =Cov[x(0) ]  .  (2.16) 


The  Q  matrix  is  the  covariance  of  the  state  excitation 
noise.  If  the  disturbances  to  the  system  are  uncorrelated, 
the  Q  matrix  is  diagonal.  The  Q  matrix  accounts  for  the  noise 
processes  in  the  system  as  well  as  modeling  errors  between 
model  and  system  dynamics. 

R  matrix  is  the  covariance  matrix  of  measurement  error. 
Large  values  of  R  means  that  the  measurements  are  not 
consistent  with  the  dynamic  model. 

The  matrices  Q  and  R  represent  a  trade-off  between  the 
model  and  the  observation  accuracies.  In  particular  if  the 
entries  of  R  are  larger  than  the  entries  of  Q,  it  means  that 
the  model  is  more  reliable  than  the  observations,  and  the 
Kalman  Filter  will  give  less  emphasis  to  the  observation.  Vice 
versa,  if  the  entries  of  R  are  small  compared  to  Q  ,  it  means 
that  the  observations  are  more  reliable  than  the  model,  and 
the  Kalman  filter  puts  more  emphasis  on  the  observations. 

While  the  matrix  R  is  in  general  determined  by  sensing 
device,  the  determination  of  Q  is  more  difficult  due  to  its 
lack  of  physical  meaning,  and  it  is  usually  set  by  trial  and 
error. 

As  mentioned  above  for  the  implementation  of  the  filter 
algorithm  we  need  a  priori  information  for  both  the  states  and 
the  error  covariance.  Large  values  in  the  initial  estimate  of 
P  means  that  the  filter  will  not  solely  depend  on  the  initial 
conditions  and  gives  more  emphasis  to  the  measurements. 


B.   EXTENDED  KALMAN  FILTERING 

The  Kalman  filter  yields  a  statistically  optimal  estimate 
for  the  states  of  a  LTI  system.  Unfortunately  many  real 
systems  such  as  the  model  studied  in  this  thesis  are  nonlinear 
in  nature.  A  general  nonlinear  system  driven  by  both  a 
deterministic  and  random  input  is: 

x(k+l)  =f(x(k)  ,u(k)  ,w(k)  ,k)  ,  (2.17) 

where  w(k)  is  a  random  forcing  function  with  covariance  Q, 
u(k)  is  a  deterministic  input  and  f(.)  is  a  nonlinear 
function.  The  measurement  process  can  be  modeled  as: 

y(k)=g(x(k)  ,v(k)  ,k)  ,  (2.18) 

where  v(k)  is  measurement  noise  with  covariance  R  and  g(.)  is 
a  nonlinear  function. 

Our  task  is  to  find  the  linear  estimate  of  x(k),  which 
minimizes  the  mean  square  estimation  error.  Solution  to  this 
problem  is  given  by  the  conditional  expectation.  But 
generating  and  maximizing  the  conditional  expectation  is  a 
formidable  task  which  is  not  suitable  to  on-line 
implementations . 

A  simpler  and  of  course  mostly  used  approach  is  to  use  an 
extension  of  the  Kalman  filter  by  using  Taylor  series 
expansion.  The  resulting  filter  is  suboptimal  and  does  not 
give  a  guaranteed  convergence  as  the  Kalman  filter  does.  In 
spite  of  this, the  Extended  Kalman  Filter  (EKF)  is  used  in  a 
wide  range  of  applications,  especially  in   target  tracking 
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systems  where  the  nonlinearity  stems   form  the  geometry 
involved  in  the  measurement  process. 

The  extended  Kalman  filter  equations  are  similar  to  the 
Kalman  equations: 

x(k+i\k)  =f(x(k\k)  ,u(k)  ,Q,k)  ,  (2.19) 

y{k+l\k)  =g(x(k+l\k)  ,  0 , k) ,  (2.20) 

x(k+l\k+l)  =Jt(k+i\k)  +G{k+1)  [y(k+D  -?{k+i\k)  ]   (2.21) 

As  can  be  seen  from  these  equations  the  states  are 
predicted  ahead  using  the  estimated  current  state  and  the 
known  input.  Since  the  noise  is  unknown,  it  is  set  to  its 
expected  value  of  zero.  The  Kalman  gains  are  computed  by  using 
the  first  order  linear  approximation  of  the  nonlinear  model 
using  Taylor  series  expansion.  These  approximations  are  as 
follows : 

^_df(x(k)  ,u(k)  ,w(k)  ,k)  |   ,w        .w  irs    _ 

<P 5 — 77I \x(k)=x(k\k)  (2.22) 

w(k)  =0 


A.df(x(k)  ,u(k)  ,w(k),k)  ,  fM  „_,  ##1   _x 

A 5 — m \x(k)=x(k\k)  (2.23) 

*(*) =0 


g8W),y),*)  |xW^W  (2.24) 


D=— ^ —  ,    ',  ,  ' — — — -  x(Jt)=i?(icic)  .  (2.25) 

cv^/c)  v(jt)=o 


These  linearized  values  are  used  in  the  gain  equations: 

p./c+l|Jt)  =4>P(k\k)$T+AQ£LT  (2.26) 

G{k+1)  =P(k+l\k)  +HT[HPHT+DRDT]  ~x  (2.27) 

P{k+l\k+l)  =  [I-G(k+1)H]  P{k+l\k)  (2.2  8) 

It  is  easy  to  see,  from  these  equations,  that  the  Kalman 
gains  should  be  computed  on-line  since  the  linearized  model 
depends  on  the  current  state  estimate. 

C.   SMOOTHING  ALGORITHM 

Smoothing  is  a  procedure  that  uses  all  the  state 
estimates  and  associated  error  covariances  produced  by  the 
Kalman  Filter  and  attempts  to  improve  the  accuracy  of  these 
estimates . 

Let  k  be  within  the  time  interval  0  to  N,  that  is,  OsksN, 
for  some  fixed  N.  The  Kalman  filter  estimate  at  time  k  denoted 
by  x(k+ljk)  is  based  only  on  the  measurements  up  to  time  k. 
The  smoothed  estimate  is  based  on  the  measurements  that 
occurred  over  the  entire  time  interval.  The  smoothed  estimate 
is  denoted  by  x(kJN)  and  the  smoothed  error  covariance  is 
given  by  P (k|N) . 

Meditch,  [Ref.  4],  classifies  the  smoothing  algorithms 
into  three  categories: 


10 


Fixed  interval  smoothing,  denoted  by  x(kJN)  where 
k=0 , 1 . .N- 1 ;N  is  a  positive  integer. 

Fixed  point  smoothing,  denoted  by  x(kjj)  where 
j =k+l , k+2 , . . k  and  k  is  a  fixed  integer. 

Fixed      lag      smoothing,       denoted   by  x(k|k+L),   where 

k=0,l,..N,  is  a  positive  integer,  and  L  is  the  fixed  time  lag. 

In  this  study  we  use  a  fixed  interval  smoothing  algorithm 

to  smooth  the  estimates  of  the  extended  Kalman  filter.  The 
equations  used  in  the  smoothing  algorithm  are  given  below: 

A(k)  =P(k+l\k+l)  $TP-1  (k+l\k)  (2.29) 

x(k\N)    =  x(k+l\k+l)    +  A{k)  [x(k+l\N)  -x{k+l\k)  ]       (2.30) 

P(k\N)    =  P(k+l\k+l)  +A(k)  [P(k+l\N)  -P(k+l\k)  }A(k)T       (2.31) 

As  can  be  seen  from  the  above  equations,  the  smoothed 
estimate  is  the  Kalman  filter  estimate  x(k+l|k+l) ,  adjusted  by 
a  weighted  error  term.  This  error  is  the  difference  between 
the  smoothed  estimate  and  the  predicted  estimate  calculated  by 
the  Kalman  Filter.  The  smoothed  error  covariance  has  no  impact 
in  the  smoothing  algorithm  but  it  is  a  measure  of  how  well  the 
smoothing  filter  is  working.  If  P (k j N) sP (k+1 J k+1)  this  means 
the  smoothed  estimate  is  better  than  or  equal  to  its  filtered 
estimate . 

The  fixed  interval  smoothing  algorithm  is  an  off-line 
procedure  that  operates  backward  in  time.  The  smoothing  filter 
is  initialized  by  the  last  filtered  estimate,  that  is  to  say 
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the  last  filtered  estimate  becomes  the  first  smoothed 
estimate.  It  is  also  clear  that,  in  order  to  apply  the 
smoothing  algorithm,  the  values  of  x(k+ljk),  x(k+ljk+l), 
P(k+l|k),  P(k+ljk+l)  must  be  stored  in  computer  prior  to  fixed 
interval  smoothing. 

D.   PARAMETER  ESTIMATION 

In  this  section  we  present  an  algorithm  to  estimate  the 
system  parameters  using  least  squares  method.  System 
identification  is  mainly  concerned  with  fitting  a  dynamic 
model  to  the  measured  input  and  output  data.  Trying  all 
possible  dynamic  models  is  certainly  a  formidable  task, 
therefore  we  restrict  our  models  to  a  certain  model  structure. 
We  consider  the  following  difference  equation: 

A{z-X)y{k)  =B(z~1)  u(k)  +  C(z~l)  e{k)  ,        (2.32) 

where  e(k)  is  white  noise  and  y(k)  and  x(k)  are  output  and 
input  sequences  respectively.  A,  B,  C  are  polynomials  in  the 
time  delay  operator  expressed  as: 

Aiz'1)  =  l+a1z~1  + +anz~n 

Biz'1)^     b^z~x  + +bnz~n  (2.33) 

C{z'1)  =l+c1z"1+ +cnz~n. 

This  input  output  model  is  called  ARMAX,  which  stands  for  Auto 
Regressive  Moving  Average  Extended  [Ref .  5]  .  A  particular  case 
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of  this  model  is  the  ARX  model  which  is  used  in  this  thesis. 
It  comes  from  the  ARMAX  model  with  the  moving  average  part 
removed  and  expressed  as: 

A{z-l)y(k)  =B{z'l)  u(k)  +e(k)   .  (2.34) 

We  can  write  this  in  regression  form  as: 

y[k)  =<J>T(£)£+e(/c)  ,  (2.35) 

where 


<J)r=[-y(ic-i) ,-y(k-n)  ,u(k-l)  , u(k-n)} 

§-[a1...anb1...bnV. 


(2.36) 


The  basic  method  to  estimate  the  parameters  0  is  the  least 
squares  method  which  minimizes  the  mean  square  estimation 
error: 

M 


&=argminY  \y(k)  -<t>rfi|2,  (2.37) 


with  respect  to  .0.  The  solution  to  this  problem  is  given  by 
pseudo  inverse  matrix: 

£=(<&T<I>)  -Wy,  (2.38) 

where 
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*=[<|>(1)  ,<j)(2)  , <|)(M)] 

y=[y(l)  ,y{2)  ,  .  .  .,y[M)]T, 


(2.39) 


with  M  being  the  number  of  data  points  used  in  the  parameter 
estimation. 

If  we  consider  a  first  order  difference  equation  of  the 

form: 


y(k)  +ay(k-l)  =bu(k-l)  +  e(k) 


(2.40) 


Then  using  Equations  (2,38)  and  (2,39)   the  least  squares 
estimate  of  the  parameters  a  and  Jb  are  given  as  follows: 


TTy(k-i)2  -Ty(k-i)u(k-i) 

Jc=l  Jc=l 

M  M 

-Vy(Jt-l)u(Jt-l)  ^u(k-i)2 


-Ty(k-i)y{k) 

k=l 
H 

Y,  u(k-l)y(k) 


(2.41) 


Using  either  the  pseudo  inverse  matrix  form  or  the 
summation  form,  the  parameters  a  and  b  can  be  estimated  using 
the  above  equations. 

E.   SYSTEM  MODELING 

Our  study  in  this  thesis  is  concerned  with  the  planar 
motion  of  the  AUV,  therefore  system  modeling  for  the  planar 
motion  is  considered.  The  system  model  used  in  this  study  is 
the  same  as   in  [Ref .  6] . 
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In  modeling  the  planar  motion  of  the  AUV  we  use  a  earth 
fixed  cartesian  coordinate  frame.  We  define  the  states  of  the 
vehicle  X  e  R5  as : 


X(t) 


x(t) 

y(t) 

v(t) 

0(t) 

e(t) 

(2.42) 


The  states  x  and  y  define  the  x  and  y  position  of  the  AUV.  The 
state  v  is  the  forward  velocity  of  the  AUV  and  the  states  8 
and  8   are  the  heading (yaw)  and  heading  rate  of  the  vehicle. 

The  differential  equations  describing  the  model  are  as 
follows : 

x=vcos6 
y^vsinQ 


v=-a1v+a2ul 


(2.43) 

where  a,,  a2/  b, ;  b;  are  constants  depending  on  the  vehicle 
dynamics  and  the  inputs  u1;  u2  are  the  RPM  and  rudder  commands 
to  the  system  respectively. 

The  system  model  can  now  be  expressed  in  state  space  form 
as : 
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0 

0 

COS0 

0 

0 

0 

sinG 

0 

0 

0 

-*i 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0   " 

'0      0' 

0 
0 
0 

x+ 

0      0 
a2    0 
0     0 

• 

U2. 

\ 

0    b2 

+  w, 


(2.44) 


where  W  is  zero  mean  gaussian  noise  which  accounts  for  the 
errors  between  the  model  and  the  actual  system  dynamics. 
Notice  that  this  model  does  not  include  side-slip  since  we 
assume  the  velocity  vector  to  be  in  line  with  the  heading  of 
the  vehicle.  For  simplicity,  the  error  due  to  side  slip  is 
taken  into  account  by  the  noise  term  W. 

In  the  next  chapter  we  are  going  to  use  this  model  in 
conjunction  with  the  model  of  the  environment  to  determine  the 
optimal  state  estimator. 
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III.  APPLICATION  OF  POTENTIAL  FUNCTIONS 

A.   POTENTIAL  FUNCTION  THEORY 

Autonomous  underwater  vehicles  must  have  the  ability  to 
locate  themselves  in  partially  known  environments  using 
sensors  available  to  them.  In  this  study,  we  address  the 
problem  of  navigating  the  Autonomous  Underwater  Vehicle (AUV) 
using  sonar  returns.  These  sonar  returns  are  incorporated  with 
a  potential  function  and  used  as  nonlinear  measurements  in 
Kalman  Filter.  The  potential  function  is  a  function  which,  in 
a  sense,  defines  the  area  where  the  vehicle  operates.  The 
basic  idea  behind  the  potential  function  is  the  following: 

i)  it  returns  a  value  of  zero  at  the  boundaries  of  the 
object  and  returns  a  value  between  one  and  zero  elsewhere; 

ii)  its  derivative  is  maximum  at  the  boundaries  of  the 
object . 

As  an  example,  if  we  consider  a  pool  of  rectangular  shape 
with  sides  L,  and  L2  we  can  define  a  function  |8(x,y)  such  that: 

P(x,y)  =x(x-L1)y{y-L2)  ,  (3.1) 

where  we  take  the  lower  left-hand  corner  of  the  pool  as  the 
origin  of  the  reference  frame.  If  the  sonar  return  is  from  the 
sides  of  the  pool  it  returns  a  value  of  zero.  We  combine  the 
defining  function  with  a  squashing  function  to  provide 
boundedness  and  define  the  potential  function  as: 
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V(x,y)    =  Fk{$(x,y)  )  .  (3.2) 

The  function  Fx(z)  must  be  smooth,  bounded  and  dif f erentiable . 
A  possibility  is  to  choose  Fx(z)  to  be  a  sigmoid  function: 

Fk(z)    =    x  e  -  ,  (3.3) 

with  X  being  a  strictly  positive  parameter. 

The   potential   function   must   satisfy   the   following 

requirements : 

i)  V  should  be  continuous  and  dif f erentiable . 

ii)  V(Xo,y0)  should  return  a  value  of  zero  if  the  return  is 
from  a  reflecting  surface. 

iii)  V  should  be  uniformly  bounded. 

Note  that  the  first  and  the  third  requirements  are  met  by 
the  sigmoid  function,  while  the  second  is  met  by  the  defining 
function  #.  The  plot  of  a  sigmoid  function  for  various  values 
of  X  is  given  in  Figure  (3.1) .  The  function  is  continuous  and 
bounded  over  the  interval  [-1,  1]  and  as  the  parameter  X 
decreases,  the  "sharpness"  of  the  function  increases. 

If  we  consider  a  field  of  operation  containing  several 
obstacles  0, ,  02, . . . ,  0n,  each  of  which  has  elliptical  shape, 
we  can  describe  a  defining  function  for  each  obstacle  as: 


P,  =[*y]  a,  y-Cj,  (3*4) 
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where  A,  e  R2x2  and  C,  e  R  [Ref .  7]  .  The  total  defining  function 
is  the  product  of  each  function: 

PU,y)  =  JlPj(^y)  •  (3.5) 

j 

As  an  example,  if  we  want  to  add  a  buoy  of  cylindrical  shape 
of  radius  r  at  point  (a,b)  in  the  pool  the  defining  function 
will  be: 

Pobst(^y)  ={x-a)2*(y-b)2-z2,  (3.6) 

and  the  total  defining  function  will  be: 

PtoC(x,y)=  x(x-Lx)y{y-L2)   (  (x-a)  2  +  (y-b)  2-i2)  )  ,    (3.7) 
where  the  overall  potential  function  is: 

V(x,y)  =  Fk(Ptot(x,y))   .  (3.8) 

By  following  the  above  procedure,  additional  obstacles  may  be 
added  to  the  environment. 

The  parameter  X  has  a  vital  role  in  shaping  the  potential 
function,  the  small  values  in  X  result  in  a  steeper  potential 
function  surface  while  larger  values  result  in  a  smoother 
surface.  In  Figure  (3.2)  we  see  the  3D  plot  of  a  potential 
function  in  the  pool.  It  is  easily  seen  that  at  the  pool 
borders  it  gives  a  value  of  zero,  whereas  at  the  other  points 
it  returns  a  value  between  zero  and  one. 
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Figure  3.1  Sigmoid  Function  for  various  values  of  X 


Figure  3.2  Potential  function  for  the  pool 
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B.   APPLICATION  TO  SYSTEM  MODEL 

The  system  model  in  this  study  has  the  form  of  Equation 
(2.44).  The  vehicle  states  are  position  in  x  and  y 
coordinates,  velocity,  heading (yaw)  and  heading  rate 
respectively.  The  navigation  algorithm  which  is  based  on  the 
extended  Kalman  Filter  uses  the  sonar  range  measurements. 
Therefore  we  need  to  define  a  measurement  equation  which 
relates  the  states  of  the  vehicle  to  the  sonar  range 
measurement.  The  measurements  available  to  the  system  are 
sonar  ranges  p  at  angles  a  with  respect  to  the  longitudinal 
axis  of  the  vehicle.  We  define  the  location  of  the  "tip"  of 
the  sonar  range  as 

[xO,yO]  =  (x+pcos  (6  +  a),y+psin(6  +  oO),        (3.9) 

where  x,  y,  and  8  are  the  current  position  and  heading  of  the 
vehicle . 

We  relate  the  sonar  range  measurement  to  the  system  model 
via  the  potential  function  so  that  the  measurement  equation 
becomes : 

y=g{X,  p,  a) =  ^(x+pcos (6  +  a) ,y+psin(0+a) )  +W1,        (3.10) 

where  the  function  g  is  a  nonlinear  function  of  the  states  and 
the  sonar  measurement  and  Wl  is  zero  mean  gaussian  measurement 
noise.  The  function  g  is  the  potential  function  which  defines 
the  area  of  operation.  Apart  from  measurement  noise  the 
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measurement  equation  should  have  the  value  of  zero  if  the 
sonar  return  is  consistent  with  the  map  of  the  area. 

By  defining  the  above  measurement  equation  we  can  write 
the  overall  model  in  the  standard  state  space  form  as: 

X=AX+Bu+W 

(3.11) 

y=g{X,p,a)  +W1 
The  system  has  a  nonlinear  state  equation  and  a  nonlinear 
observation  equation.   In  order  to  apply  extended  Kalman 
filtering  we  need  to  get  the  linearized  observation  matrix  H 
as  in  Equation (2 , 24) ,  which  is: 

g.agt*.p,a).tjg,  jg,  0,  |g  ,0],        <3.12) 

dx  dx      dy  do 

The  partial  derivatives  are  found  by  application  of  the  chain 

rule : 

dg^^f\dz  dg=dF\dz 

dx     dz  dx  dy     dz  dy 

(3.13) 

dg  _  dFk  dz  dx    d&\  dz  dy 

ae    dz  dx  ae    dz  dy  ae ' 

where  the  function  Fx  (z)   is  the  sigmoid  function  as  in 
Equation (3,3) . 

C.   SIMULATIONS  IN  A  RECTANGULAR  POOL 

The  estimation  algorithm  was  tested  first  in  a  pool  of 
size  12X12  feet.  The  Tritech  ST725  high  resolution  sonar 
onboard  NPS  AUVII  was  simulated.  The  sonar  has  a  rotating  head 
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with  a  scan  rate  of  nine  seconds  and  yields  400  sonar  returns 
per  scan  corresponding  to  0.8  degrees  per  return.  The  data 
from  seven  successive  scans  of  the  sonar  head  are  processed  by 
the  Kalman  filter.  At  point  (9,9)  in  the  pool  we  placed  a 
buoy  of  radius  0.5  feet,  and  the  potential  function  becomes: 

V(x,y)  =Fk(  [x(x-12)y(y-12)  ]  [  (x-9  )  2+  (y-9  )  2-0  .  25]  )  .   (3.14) 

In  Figure  (3.3)  the  position  estimation  with  known  initial 
location  is  shown.  The  location  of  the  sonar  tip, 
corresponding  to  the  estimated  borders  of  the  pool  is  plotted 
for  each  of  the  seven  successive  sonar  scans  and  the  contour 
of  the  potential  function  is  superimposed.  In  the  last  plot 
the  estimated  trajectory  of  the  vehicle  is  shown.  The 
estimated  trajectory  of  the  vehicle  matches  the  actual 
trajectory. 

The  parameter  X  plays  an  important  role  in  the  estimation 
algorithm.  Its  value  should  be  kept  high  enough  to  correct  for 
errors  in  the  initial  estimates  and  low  enough  to  discriminate 
objects  which  are  not  on  the  map.  The  filter  uses  a  time 
varying  X  which  decreases  with  time  so  that  the  potential 
function  surface  get  steeper  as  the  estimates  converge  to  the 
actual  values.  When  there  is  not  enough  information  in  the 
initial  position  of  the  vehicle  the  filter  starts  with  a  large 
value  of  X  to  account  for  the  errors  in  the  initial  estimates. 

In  Figure  (3.4)  result  of  the  estimation  with  unknown 
initial  position  is  shown.  The  estimated  trajectory  converges 
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to  the  actual  trajectory.  The  effects  of  the  time  varying  X 
can  be  seen  by  examining  the  contours  of  the  potential 
function.  Figure  (3.5)  shows  the  result  of  estimation  where  we 
have  an  error  in  the  initial  heading  as  well  as  the  initial 
position.  The  system  is  more  sensitive  to  heading 
perturbations,  however,  the  estimation  converges  to  the  actual 
trajectory.  Figure  (3.6)  shows  the  result  of  simulation  for 
the  case  when  the  obstacle  at  point  (9,9)  is  not  defined  in 
the  potential  function.  The  uncharted  obstacle  is  detected  by 
the  algorithm  and  the  actual  trajectory  is  recovered  from  the 
estimation. 

In  Figures  (3.7)  and  (3.8)  we  repeat  the  experiment  with 
two  cylindrical  buoys  placed  at  locations  (2,2)  and  (9,9)  in 
the  pool.  In  Figure  (3.8)  the  obstacles  are  not  defined  in  the 
potential  function.  Results  of  estimation  show  that  the 
algorithm  proves  to  be  robust  in  the  presence  of  uncharted 
obstacles . 
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Figure  3.3  Position  estimation  with  known  initial  location, 
one  obstacle  present. 
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Figure  3.4  Estimation  with  unknown  initial  position,  one 
obstacle  present. 
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Figure  3.5  Estimation  with  an  error  in  initial  position  and 
heading,  one  obstacle  present. 
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Figure  3.6  Estimation  with  unknown  initial  position,   the 
obstacle  is  not  defined  by  potential  function. 
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Figure  3.7  Estimation  with  unknown  initial  location,   two 
obstacles  present. 
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Figure  3.8  Two  obstacles  present,  unknown  initial  location, 
the  obstacles  are  not  defined  by  potential  function. 
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D.   SIMULATIONS  IN  AN  ENVIRONMENT  WITHOUT  BORDERS 

After  testing  the  algorithm  in  a  pool  we  proceed  further 
to  investigate  its  behaviour  in  an  environment  without 
borders,  such  as  the  open  sea.  In  this  case  the  potential 
function  only  defines  the  obstacles  in  the  area  of  operation. 
For  the  case  when  one  obstacle  of  cylindrical  shape  is 
present,  the  potential  function  will  be: 


V[x,y)    -   ix-a)z  +   {y-b)2   -r2 


(3.15) 


It  returns  a  value  of  zero  if  the  sonar  range  return  is. from 
the  charted  obstacles.  A  3D  plot  of  the  potential  function, 
for  the  case  of  one  obstacle   is  shown  in  Fiaure  (3.9)  . 


Potential  Function  When  One  Obstacle  is  present 


y(in  feet) 


0  0 


x(\n  feet) 


Figure  3.9  Potential  Function  when  one  obstacle  is  oresent 


31 


In  generating  data  for  this  simulation  a  high  value  of 
sonar  range  is  given  unless  the  sonar  return  is  from  an 
obstacle.  We  also  processed  the  data  from  seven  successive 
sonar  scans  using  a  time  varying  A  parameter.  Figure  (3.10) 
shows  the  estimation  when  there  is  an  obstacle  of  cylindrical 
shape  at  point  (5,5)  in  the  environment.  We  also  assume  to 
have  exact  information  of  the  initial  location  of  the  vehicle. 
As  we  notice,  the  actual  and  estimated  trajectories  coincide. 
In  Figure  3.11  we  show  the  position  estimation  when  there  is 
an  error  in  the  initial  position  of  the  vehicle.  In  this  case 
the  estimated  trc  zctory  eventually  converges  to  the  actual 
trajectory.  In  the  second  simulation  we  included  three 
cylindrical  buoys  at  locations  (5,5),  (15,15),  (18,5)  so  that 
the  potential  function  description  is: 


$x(x,y)  =(x-5)2+(y-5)2-0.25 
P2(x,y) =(x-15)2+(y-15)2-0.25 
P3  (x,  y)  =  (x-18)  2+  (y-5 )  2-0  .  25 
^(x,y)=Fx(P1(x,y)p2(x,y)P3(x;y)) 


(3.16) 


A  3D  plot  of  the  potential  function  is  shown  in  Figure 
(3.12)  .  The  result  of  the  estimation  is  shown  in  Figure  (3. 13)  . 
In  Figures  (3.14)  and  (3.15)  we  included  two  extra  obstacles 
at  points  (3,10)  and  (8,20)  which  are  not  included  in  the  map 
of  the  environment.  In  both  cases  the  algorithm  determines  the 
obstacles  that  are  not  on  the  map  and  marks  them  on  the 
subsequent  plots. 
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Figure  3.10  Position  estimation  with  known  initial  locations, 
one  obstacle  present. 
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Figure  3.11  Position  estimation  with  unknown  initial  position, 
one  obstacle  present. 
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Potential  Function  When  Three  Obstacles  are  Present 
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Figure  3.12  3D  plot  of  potential  function 
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Figure  3.13  Estimation  with  unknown  initial  position,  three 
obstacles  present. 
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Figure  3.14  Obstacles  that  are  not  on  the  map  are  marked, 
known  initial  position. 
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unknown  initial  position. 
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E.   THE  EFFECTS  OF  CURRENTS 

Currents  present  in  the  area  of  operation  is  one  of  the 
main  contributions  to  the  uncertainty  in  the  position  of  the 
vehicle.  The  estimation  algorithm  should  be  modified  to 
accommodate  the  effects  of  currents  in  the  navigation 
algorithm.  The  inclusion  of  currents  leads  to  the  addition  of 
two  extra  states  in  the  system  model.  We  further  assume  the 
currents  to  be  constant  or  slowly  varying.  The  two  extra 
states  are  Cxand  Cy  which  are  associated  to  the  components  of 
the  currents  in  the  x  and  y  directions  respectively,  such 
that: 


cx=o 


cy-o. 


(3.17) 


The  system  states  becomes  X  e  R7  and  the  system  model  is 
represented  as: 


X  = 


0  0  cosG  0 
0  0  sin6  0 


0  0 

0  0 

0  0 

0  0 

0  0 


-a, 
0 
0 
0 
0 


0 

1  o" 

r  0 

0  ' 

0 

0    1 

0 

0 

0 

0    0 

a2 

0 

1 

0    0 
0    0 

X  + 

0 

0 

0 

ul 

u2_ 

0 

0    0 

0 

0 

0 

0    0. 

0 

0. 

+  w. 


(3.18) 


In  Figure  (3.16)  the  effects  of  currents  which  cause  a 
significant  amount  of  offset  in  the  final  position  of  the 
vehicle  is  shown.  Figure  (3.17)  and  Figure  (3.18)  show  the 
estimation  for  known  and  unknown  values  of  the  initial 
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position  assuming  the  currents  are  known.  We  further  tested 
the  algorithm  for  the  case  when  we  do  not  have  any  information 
about  the  currents  in  the  area  of  operation,  which  might 
generally  be  the  case.  Figure  (3.19)  gives  the  results  of 
position  estimation,  which  shows  a  reliable  estimate  in  the 
presence  of  unknown  currents. 

In  Figures  (3.20)  and  (3.21)  we  repeat  the  experiment  for 
the  case  when  there  are  two  obstacles  in  the  area.  For  both 
cases  the  navigation  algorithm  is  able  to  recover  the  actual 
trajectory. 
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Figure  3.16  The  effects  of  currents  in  the  trajectory  of  the 
vehicle. 
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Figure  3.17  Position  estimation  for  known  initial  location, 
three  obstacles  present. 
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Figure  3.18  Position  estimation  with  unknown  initial  location, 
three  obstacles  present. 
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Figure  3.19   Position  estimation  with  unknown  initial  position 
and   current,    three   obstacles   present. 
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Figure  3.20  Position  estimation  with  unknown  initial  position 
and  currents,    two  obstacles  present. 
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Figure  3.21  Position  estimation  with  unknown  initial  position, 
two  obstacles  present. 


For  the  simulations  in  this  Chapter  we  have  applied  Least 
Squares  (LS)  parameter  estimation  techniques  to  estimate 
dynamic  parameters  a,,  a2,  b,,  b2  associated  with  Equation 

(2.44)  . 

The  results  of  the  parameter  estimation  are  given  in  Table 

(3.1).  The  actual  values  are  used  in  data  generation  for  the 
computer  simulations.  The  table  shows  the  mean  of  the 
estimated  parameters  for  six  different  simulations.  The 
percentage  error  made  in  the  estimates  are  less  than  one 
percent  which  is  within  acceptable  limits. 
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Table  3.1  ESTIMATED  DYNAMIC  PARAMETERS  BY  LEAST  SQUARES 


Dynamic 
Parameter 

Actual  Value 

Estimated 
Value 

Percentage 
Error 

a, 

1.0000 

0.9977 

0.23  % 

a2 

0.0010 

0.0010 

0.0   % 

b, 

1.0000 

1.0002 

0.02  % 

b2 

1.0000 

1.0001 

0.01  % 

F.   STUDIES  WITH  ACTUAL  DATA 

In  this  section  we  apply  the  navigation  algorithm  using 
actual  data.  The  data  is  collected  on  a  water  tank  of  size  6x6 
meter.  The  data  consists  of  a  set  of  sonar  returns  including 
range  p  and  bearing  oi.  The  sonar  measurements  are  taken  by  a 
Tritech  725  high  resolution  sonar  onboard  the  NPS  AUVII.  Five 
successive  sonar  scans  are  processed  by  the  Kalman  filter. 
Each  sonar  scan  consists  of  200  range  measurements 
corresponding  to  a  sector  of  1 . 8  degrees  per  sonar  return.  The 
typical  sonar  scan  of  the  pool  is  shown  in  Figure  (3.22)  .  Due 
to  gain  problems  in  the  sonar,  the  range  measurements  are 
excessively  corrupted  by  noise.  We  see  these  effects  in  the 
sonar  scan  of  Figure  (3.22)  ,  even  though  there  are  no  objects 
inside  the  watertank  we  have  sonar  readings  inside  the  tank  as 
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well  as  the  outside  of  it.  In  collecting  data  for  the 
experiment,  no  RPM  or  rudder  command  was  given  to  the  vehicle 
and  constant  speed  and  heading  was  used.  Therefore,  we  used  a 
simpler  version  of  the  system  model.  In  this  case  our  state 
vector  is  X  e  R4,  given  by  x  and  y  position,  velocity  and 
heading.  This  results  in  a  state  space  form  of  : 


0  0  cos0  0 

0  0  sin0  0 

0  0   0  0 

0  0   0  0 


x+w, 


(3,19) 


where  W  is  zero  mean  gaussian  white  noise.  In  this  case  we  do 
not  have  any  obstacles  in  the  tank  so  the  potential  function 
will  only  define  the  borders  of  the  pool  such  that: 


V(x,y)  =Fk(x{x-6)y(y-6)  ) 


(3,20) 


The  range  and  bearing  values  from  each  successive  sonar 
return  are  fed  to  the  Kalman  Filter  and  the  parameter  X  is 
reduced  by  50  percent  at  each  complete  scan.  The  position 
estimation  for  known  initial  location  is  given  in  Figure 
(3.23)  .  In  the  subsequent  plots,  estimated  borders  of  the  pool 
for  each  successive  sonar  scan  are  plotted  along  with  the 
contours  of  the  potential  function.  Also  the  estimated 
trajectory  of  the  vehicle  is  shown.  The  readings  inside  the 
pool  are  due  to  the  gain  problems  associated  with  the  sonar  as 
mentioned  before.  In  Figure  (3.24)  the  position  estimation 
with  unknown  initial  condition  is  shown,  where  we  have  the 
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center  of  the  tank,  namely  the  point  (3,3)  as  an  initial 
estimate.  The  estimation  converges  to  the  actual  values. 

The  fixed  interval  smoothing,  as  explained  in  Chapter  II, 
is  an  off-line  procedure  that  uses  the  state  estimates  and 
associated  error  covariances  to  smooth  the  estimates  produced 
by  the  Kalman  Filter.  In  order  to  overcome  the  transient 
problems  in  the  filter  and  get  better  estimates  about  the 
position  of  the  AUV  we  applied  the  smoothing  algorithm  to  the 
Kalman  filter  output.  In  applying  the  algorithm  we  stored  the 
state  estimates  and  the  error  covariances  for  each  scan  and 
fed  these  values  to  the  smoothing  filter.  The  smoothing  filter 
runs  backwards  in  time  and  the  final  state  estimate  and  the 
error  covariance  of  the  Kalman  filter  become  the  a  priori 
estimate  and  the  error  covariance  of  the  smoothing  filter. 

In  Figures  (3.25)  and  (3.26)  results  of  the  estimation 
with  a  smoothing  filter  are  shown.  The  smoothed  trajectory  is 
given  in  the  last  plot.  It  is  easily  seen  that  fixed  interval 
smoothing  provides  better  state  estimates  and  does  not  suffer 
the  transient  problems  associated  with  the  Kalman  filter.  In 
Figure  (3.27)  we  applied  the  Fixed  Interval  Smoothing 
algorithm  to  the  computer  generated  data.  The  main 
disadvantage  of  the  smoothing  algorithm  is  having  to  store  a 
large  amount  of  data,  which  may  cause  memory  overflow  in  the 
computer  of  the  AUV  studied  in  this  thesis. 
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Figure  3.22  Typical  sonar  scan  in  the  pool 
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Figure   3.23    Position   Estimation  with  known   initial    location 
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Figure  3.24  Position  estimation  with  unknown  initial  location 
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Figure  3.25  Fixed  interval  smoothing,  known  initial  location 
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Figure  3.26  Fixed  interval  smooching,  unknown  initial  location 
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Figure  3.27  Fixed  interval  smoothing  applied  to  computer 
generated  data. 
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IV.  NEURAL  NETWORK  MODELS  FOR  POTENTIAL  FUNCTIONS 

A.   GENERAL 

A  neural   network   consists   of   many   simple   elements 
operating  in  parallel.  It  can  be  trained  to  perform  complex 
tasks  involving  system  identification  and  classification, 
control  systems,  vision  and  pattern  recognition  [Ref .  8] . 

Neural  networks  are  composed  of  neurons .  Each  neuron  in 
the  net  is  made  of  weighted  inputs,  a  bias  and  a  transfer 
function.  The  input  to  a  neural  network  is  multiplied  by  its 
weight  and  summed  by  the  bias  b.  The  role  of  the  bias  is  to 
shift  the  argument  of  the  transfer  function  by  a  corresponding 
amount.  The  model  for  a  neuron  is  given  in  Figure  (4.1) . 


Figure  4.1  Compact  notation  for  the  neuron  model 
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In  this  representation  R  represents  the  number  of  inputs  to 
the  neuron  and  Q  is  the  number  of  input  vectors  which  is 
called  batch.  Two  or  more  neurons  can  be  combined  in  a  single 
layer  and  a  neural  network  may  contain  one  or  more  such  layers 
[Ref .  8] . 

When  a  neural  network  consists  of  multiple  layers  of 
neurons,  the  layer  whose  output  is  the  overall  network  output 
is  called  output  layer  and  other  layers  are  called  hidden 
layers.  An  example  of  a  two  layer  neural  network  is  given  in 
Figure  (4.2) .  In  the  hidden  layer  there  are  SI  neurons  and  in 
the  output  layer  there  are  S2  neurons . 


Figure  4.2  Two  layer  neural  network 
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The  output  at  the  hidden  layer  is  given  by: 

Al   =  Fl {W1*P+B1)   .  (4.1) 

The  overall  network  output  A  is  given  by: 

A=F2{W2*A1+B2) 

(4.2) 
A=F2{W2*F1  {W1*P+B1)  +B2)  , 

where  F2  is  the  transfer  function  of  the  output  layer,  W2  and 
B2  are  the  weights  and  biases  for  the  output  layer 
respectively. 

B.   BACKPROPAGATION  NEURAL  NETWORKS 

The  backpropagation  algorithm  is  designed  to  train  multi 
layered  neural  networks  with  different  transfer  functions  to 
perform  function  approximation  and  pattern  recognition  [Ref. 
8]  .  It  can  be  shown  that  a  backpropagation  neural  network  with 
at  least  one  sigmoid  layer  is  capable  of  approximating  any 
smooth  function. 

The  algorithm  adjusts  weights  and  biases  so  as  to  minimize 
the  sum  squared  error  between  the  network's  output  and  the 
desired  signal.  This  procedure  is  done  in  the  direction  of 
the  steepest  descent  with  respect  to  the  error  which  is  called 
gradient  descent   procedure. 

Backpropagation  neural  networks  give  reasonable  answers 
when  they  are  fed  with  inputs  they  have  never  seen.  This 
generalization  property  gives  the  convenience  of  training  the 
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network  with  fewer  input/output  pairs  instead  of  the  whole 
input/output  data  [Ref.  8]. 

In  the  backpropagation  algorithm  the  parameters  W  are 
updated  as: 

frjM  =  ^c-^L-^  (4.3) 

where  J  is  the  square  of  the  error  between  network's  output 
and  desired  signal,  and  \l  is  the  learning  rate.  If  the 
learning  rate  is  too  high  it  causes  unstable  learning,  in  a 
sense  that  the  weights  will  diverge.  If  it  is  too  small  it 
requires  very  long  training  times. 

Another  complication  with  backpropagation  is  the  problem 
of  local  minima.  This  is  caused  by  the  fact  that  the  error 
function  is  non  quadratic  in  the  weights.  It  is  possible  for 
the  estimated  parameters  to  become  trapped  in  one  of  the  local 
minima.  This  problem  can  be  addressed  by  adding  a  momentum 
term.  Momentum  makes  the  neural  network  insensitive  to  the 
small  variations  in  the  error  surface.  Therefore  it  prevents 
the  neural  network's  solution  to  converge  to  a  local  minimum 
which  causes  higher  steady  state  error. 
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C.   MODELING  OF  POTENTIAL  FUNCTIONS  BY  NEURAL  NETWORKS 

In  this  thesis  we  use  backpropagation  neural  networks  in 
the  context  of  function  approximation.  The  main  idea  behind  it 
is  to  get  approximate  values  of  the  potential  function  and  its 
derivatives  for  use  in  the  Kalman  filter.  As  stated  in  Chapter 
III,  the  nonlinear  measurement  equation  is  the  potential 
function.  The  linearized  H  matrix  is  therefore  made  of  the 
derivatives  of  the  potential  function  with  respect  to  the 
states : 

*=[!?,   |Y,   0,   <£,   0],  (4.4, 

dx        ay  3d 

When  the  system  detects  a  reflecting  surface  at  point 
(x,y)  in  the  plane,  the  network  is  trained  with  a  "zero" 
output  in  accordance  with  the  definition  of  the  potential 
function  as  in  Chapter  III. 

The  inputs  to  the  neural  network  are  the  coordinates  of 
the  tip  of  the  sonar  beam  which  is  a  two  element  input  vector. 
The  potential  function  with  its  derivatives  is  a  three  element 
output  vector  which  we  use  to  train  the  network.  These  input 
and  output  pairs  are  nonlinearly  related.  We  apply  a 
backpropagation  algorithm  for  our  network  model.  The  network 
architecture  for  this  model  is  shown  in  Figure  (4.3) . 
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Figure  4.3  Neural  Network  architecture. 

As  the  input  to  the  network  we  have  the  coordinates  of  the 
tip  of  the  sonar  beam.  In  layer  one,  nine  neurons  with  a 
tangent -sigmoid  transfer  function  are  used.  In  layer  two, 
since  our  output  size  is  three,  only  three  neurons  are  used. 

The  results  of  training  for  the  potential  function  are 
given  in  the  following  figures.  In  Figure  (4.4)  a  two  layer 
plain  backpropagation  network  is  used.  After  3  00  epochs  the 
sum  squared  error  reaches  a  minimum  value.  Figure  (4.5)  shows 
the  results  of  training  when  backpropagation  with  added 
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momentum  term  is  used.  While  the  plain  backpropagation 
converges  to  a  local  minimum,  the  momentum  makes  the  network 
converge  to  a  lower  sum  squared  error. 

The  training  time  can  also  be  decreased  by  using  an 
adaptive  learning  rate  by  optimizing  the  ji  based  on  the  error. 
Figure  (4.6)  gives  the  results  of  training  when  an  adaptive 
learning  rate  with  momentum  is  used.  The  learning  process  is 
faster;  while  it  takes  300  epochs  to  reach  a  minimum  sum 
squared  error  with  momentum  only,  the  same  error  level  can  be 
reached  within  120  epochs  by  adding  an  adaptive  learning  rate. 
Consequently  a  two  layer  backpropagation  neural  network  with 
adaptive  learning  rate  and  momentum  is  used  for  our  application. 
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Figure  4.4  Results  of  training  with  two  layer  backpropagation 
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Figure  4.6  Training  with  adaptive  learning  rate  and  momentum. 
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After  training  the  neural  network  with  input  and  output 
data  sets  as  explained  before,  the  values  of  the  weight  and 
bias  matrices  are  stored.  After  this,  the  potential  function 
is  computed  using  the  neural  network.  During  the  estimation, 
the  coordinates  of  the  sonar  tip  (Xo,y0)  ,  are  fed  to  the  neural 
network.  The  values  of  the  potential  function  and  its 
derivatives  are  taken  as  the  network's  output,  and  used  by  the 
Kalman  filter.  However,  the  training  of  the  neural  network  for 
the  case  of  unknown  initial  conditions  of  the  vehicle  was  not 
successful  due  to  the  uncertainties  in  the  problem. 

The  results  of  the  estimation  based  on  the  backpropagation 
neural  network  are  given  in  the  following  figures.  In  Figure 
(4.7)  there  is  one  obstacle  located  at  point  (15,15)  and  in 
Figure  (4.8)  there  are  two  obstacles  located  at  (5,10)  and 
(10,5)  .  In  both  cases  the  estimated  position  follows  the 
actual  trajectory  of  the  vehicle.  Finally,  in  Figure  (4.9), 
the  algorithm  is  applied  to  the  actual  data  collected  in  the 
water  tank,  which  exhibits  fairly  good  estimates  about  the 
position  of  the  vehicle. 
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Figure  4.7  Estimation  using  neural  networks,  one  obstacle 
present . 
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Figure   4.9   Estimation  using   neural   networks   with  data 
collected  in  the  water  tank. 
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V.    SUMMARY,  CONCLUSIONS  AND  RECOMMENDATIONS 

In  this  study  the  problem  of  navigating  an  Autonomous 
Underwater  Vehicle  in  a  partially  known  environment  was 
investigated.  In  the  navigation  algorithm  we  made  use  of 
potential  functions  to  define  the  area  of  operation.  The 
potential  function  was  combined  with  the  vehicle  dynamics  in 
an  extended  Kalman  filter.  Throughout  the  simulation  studies 
the  algorithm  proved  to  be  robust  even  in  the  presence  of 
obstacles  that  are  not  included  in  the  map. 

The  method  was  first  tested  in  a  rectangular  pool 
featuring  several  cylindrical  buoys  in  the  environment. 
Simulation  studies  were  performed  assuming  known  and  unknown 
initial  locations  of  the  vehicle.  Subsequently,  the  algorithm 
was  applied  in  an  open  environment,  such  as  the  open  sea, 
which  included  several  obstacles.  The  obstacles  that  were  in 
the  area  but  which  were  not  included  in  the  map  were 
identified  by  the  algorithm.  The  effects  of  currents  were  also 
sought.  Simulation  studies  showed  that  even  in  the  presence  of 
currents  the  algorithm  gave  a  reasonable  solution  to  the 
problem. 

Finally,  the  algorithm  was  applied  to  the  actual  data 
collected  in  the  water  tank,  The  results  with  the  experimental 
data  proved  that  the  algorithm  could  be  reliably  used  in  the 
implementation.  A  Fixed  interval  smoothing  algorithm  was  also 
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used  in  order  to  smooth  the  estimates  produced  by  the  Kalman 
filter.  Throughout  the  simulation  studies  the  dynamic 
parameters  were  estimated  by  least- squares  techniques  and 
these  estimated  parameters  were  used  in  the  filter. 

In  Chapter  IV  a  neural  network  was  designed  for  use  in  the 
navigation  algorithm.  Basically,  the  potential  function  was 
replaced  by  a  backpropagation  neural  network.  This  approach 
was  successful  only  in  the  case  of  a  known  initial  location. 

The  simulation  studies  using  both  computer  generated  and 
the  actual  data  show  that  the  navigation  algorithm  could  be 
used  in  NPS  AUVII.  Several  topics  might  further  be 
investigated  such  as: 

•  Including  more  complex  features  in  the  potential 
function  so  that  a  more  precise  map  of  the  area  could 
be  used; 

•  Training  neural  networks  so  that  the  neural  network 
can  be  used  in  the  presence  of  uncertain  initial 
conditions;  and 

•  Converting   the   navigator   main   program   to   C* 
programming  language  for  use  in  the  NPS  AUVII  on  board 
computer. 
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APPENDIX 

A.   PROGRAM  STRUCTURE 

The  programming  which  covers  an  integral  part  of  this 
study  is  accomplished  using  the  MATLAB  software  package.  The 
main  program  is  NAVIGATE . M .  This  program  reads  the  data  file, 
prompts  the  user  for  the  values  of  the  initial  state 
estimates,  error  covariance  and  the  parameter  \.  There  are 
three  subroutine  calls  in  this  program.  PAREST.M  estimates  the 
dynamic  parameters  associated  with  the  system  model  using  a 
least-squares  algorithm.  EST12P.M  is  the  function  that 
commands  the  estimator;  it  feeds  the  data  to  the  Kalman 
filter,  adjusts  the  parameter  X  and  does  the  plotting 
routines.  SCAN10.M  is  the  function  that  applies  extended 
Kalman  filtering.  VP512.M  is  the  function  that  calculates  the 
potential  function  and  its  derivatives  in  the  rectangular 
pool.  SIGMOID. M  and  DSIG.M  are  the  functions  that  gives  the 
value  of  the  sigmoid  function  and  its  derivative.  SHOW10.M  is 
the  plotting  routine.  EST12P.M  ,SCAN10.M  , SIGMOID. M,  DSIG.M 
and  SHOW10.M  are  modified  from  Ref [6] . 

VP_CYL.M  is  the  function  that  gives  the  potential  function 
for  the  rectangular  pool  when  there  is  one  cylindrical 
obstacle  present.  EST_OPNP.M  is  the  function  that  commands  the 
estimator  when  there  are  no  borders.  VP  OPEN.M  and  VP  0PEN2.M 
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are  the  functions  that  give  the  potential  function  in  an 
environment  without  borders.  PL0T_0PEN2.M  is  the  plotting 
routine  for  the  potential  function. 

EST_CUR1.M  and  SCAN_CUR1.M  are  functions  used  to  model  the 
estimator  in  presence  of  currents.  EST_ACT2.M  and  SCAN_ACT2.M 
are  the  functions  that  are  used  with  actual  data  collected  in 
the  water  tank.  These  functions  also  apply  fixed  interval 
smoothing  to  Kalman  filter  estimates.  SMOOTH. M  is  the 
subroutine  that  accomplishes  the  fixed  interval  smoothing. 
This  routine  must  be  provided  previously  stored  state 
estimates  and  predictions  and  associated  error  covariances . 

TRAIN2.M  is  the  M  file  that  applies  backprogation  with 
adaptive  learning  rate  and  momentum  using  Neural  Networks 
Toolbox  of  MATLAB.  The  program  trains  the  network  given  input 
and  target  output  sets  and  generates  the  weights  and  biases 
for  the  neuron  layers.  EST_NEURO.M  and  SCNNEURO.M  are  the 
functions  that  are  used  in  neural  network  approach. 

PR_DATA6.M  and  RANGEH.M  are  the  M  files  modified  from 
Ref  [6]  which  are  used  to  generate  data  for  the  simulations  in 
the  rectangular  pool.  PR_OPEN.M  is  the  program  to  generate 
data  for  the  simulations  in  an  environment  without  borders. 
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B.   PROGRAM  LISTING 

oooooooooooooooooooooooooooooooooooooooooooooooooooo'o'o'^'o'o'^'o'o 

%navigate . m 

%  Modified  April  3rd  1994 

%  This  program  is  the  main  program  for  the  navigator 

%  The  program  prompts  the  user  for  the  values  of  state  and 

%error 

%  covariance  estimates  and  initial  value  of  the  parameter 

% lambda. 


i='Y' 

while  i=='Y' 

P=input (' Enter  initial  state  estimate:    '); 

zhO=input (' Enter  initial  error  covariance  :  '); 

lambda=input (' Enter  initial  value  of  lambda  :   '); 

%  Estimate  dynamic  parameters. 

prms=parest (data) ; 

%  Navigator  algorithm 

zhm=est_act4 (data, P, zhO , lambda, prms) ; 

%  Continue  with  another  simulation 

i=input('Do  you  want  more  simulations  ?  Y/N,'s'); 

if  isempty(i) 
i='Y' 

end 
end 


OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO 

function  zhm=estl2p (data, P, zhO , LI , L2 , lambda, prms) 

%  function  zhm=estl2p (data, P, zhO , LI, L2 , lambda, prms) 

%  modified  January  3rd  1994 

%  This  program  commands  Kalman  filter  in  the  simulations  for 

%rectangular  pool . 

%  zhO   is  5  x  l  matrix  initial  values  of  states 

%  P  is   5X5  matrix  a   priori  error  covariance  estimate. 

%  states= [x, y,v, heading, heading  dot] 

%  LI  and  L2  are  the  sides  of  the  pool. 

%  prms   is  dynamic  parameters  al,a2,bl,b2 

%  data  file  has  form: 

%  data= [alpha, rho, v, x, y, , rpm, rudder, theta, thetadot]  ; 


clg;  hold  off;clc 
n=length (data) ; 
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axis ( ' normal ' ) 
subplot (221) 
zhm= zeros (5,1) ; 
ns=400; 

for  i=  0:n/ns-l 

[zh, P, zs] =scanlO (zhO, P, data ( (ns*i+l) : (ns* (i+1) ) , :) , lambda, pr 
ms  ,  LI , L2 )  ; 

axis ( [-1  Ll+1  -1  L2  +  1] ) ; 
xm=zs(l,:);  ym=zs(2,:); 
zhm=  [zhm,  zh]  ; 

showlO (xm,ym, lambda, LI, L2) , title ( ' POOL' ) ; 
if  i  =  =  3 

meta  alpl 
end 
hold  off 
lambda  =  0 . 5  * lambda; 
nz=length ( zh) ;  zhO=zh ( : , nz) ; 

end 

hold  off 

showlO (xm, ym, lambda, LI , L2 ) ; 

hold  on 

plot (zhm(l,  : )  ,  zhm (2 ,  : )  , ' *g' )  ,  title  ( '  Estimated   Trajectory 

of  AUV' ) 

meta 

end    %estl2p 


function  [zh, P, zs] = scan 10 (zhO , P, data, lambda , prms  ,  LI ,  L2  ) 

%  modified  12  oct  93 

%  function  [zh, P, zs] =scanl0 ( zhO , P, data, lambda, prms, LI , L2 ) 

%  This  function  applies  Kalman  filter  algorithm  to  the  given 

%data 

%  returned  is  the  state  estimates  zh, estimated  borders  of 

%the  map  and  final  error  covariance. 

%  P  5  x  5  matrix, initial  covarience  matrix 

%  zhO  5x1  matrix  initial  values  of  states 

%  states= [x, y, v, heading, heading  dot]' 

%  LI  and  L2  are  the  sides  of  tthe  rectangular  pool. 

%  lambda  initial  parameter  of  potential  function 

%  prms  are  the  dynamic  parameters  al,a2,bl,b2 


n=length (data! 
zh( : , 1) =zh0; 
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R=l; 

al=prms (1) ;a2=prms (2) ;b2=prms (3) ;bl=prms (4 

Ts=0.0225; 

for  t=l:n-l 


1) 


rho=data ( t , 2 , 
alpha=data ( t , 1 
headh=zh (4 , t ) ; 
RPM(t) =data(t, 6) ; 
RUDDER (t) =data(t,7) 
a=cos (headh*pi/180) 
b=sin(headh*pi/180) 

A= [0  0  a  0  0 
0  0  b  0  0 
0  0  -al  0  0 
0  0  0  0  1 
0  0  0  0  -bl] ; 

B=[0  0;0  0;a2  0;0  0;0  b2 ] ; 
[Phi, del] =c2d(A,B,Ts) ; 
dx0=rho*cos ( (alpha+headh) *pi/180) ; 
dy0=rho*sin ( (alpha+headh) *pi/l80) ; 
x0=zh(l, t) +dx0; 
y0=zh(2 , t) +dy0; 
zs ( : , t) =  [x0;y0]  ; 

[v, dvx, dvy] =VP_cyl (xO , yO , lambda) ; 
h= [dvx, dvy, 0, ( -dvx*dy0+dvy*dx0) *pi/180, 0] ' ; 
s=h'*P*h+l;  K=Phi*P*h/s; 
e=0-v; 

zh( : , t+1) =Phi*zh( : , t) +del* [RPM(t)  RUDDER (t) ] ' +K*e; 
P=Phi*P*Phi' -K*s*K' ; 


end 
end 


%scan 


function  prms=parest (data) ; 

%  this  function  estimates  the   dynamic  parameters  of  the 

%  model  using  least  squares  estimates. 

%  the  continous  time  parameters  are  returned. 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


%estimation  for  velocity  parameters 
al=0 ; a2=0 ; a4=0 ; a5=0 ; a6=0 ; 
v=data ( : , 3) ; 
rpm=data ( : , 6) ; 

for  t=2 : length (data) ; 

al=al+ (v(t-l) ) A2; 
a2=a2+ (v(t-l) *rpm(t-l) ) ; 
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a4=a4+ ( rpm ( t - 1 )  ) A2; 
a5=a5+v(t-l) *v(t) ; 
a6=a6  +  rpm ( t - 1) *v ( t )  ; 

end 

%  discrete  velocity  parameters 

thvelo=inv(  [al  -a2;-a2  a4]  ) *  [  -a5 ; a6]  ; 

%  estimation  for   heading  parameters 
thdot=data ( : , 9) ; 
rudder=data ( : , 7) ; 
bl=0;b2=0;b4=0;b5=0;b6=0; 

for  t=2 : length (data) ; 

bl=bl+ (thdot (t-1) ) A2; 

b2=b2+ (thdot (t-1) * rudder (t-1)  )  ; 

b4=b4+ (rudder (t-1) ) x2; 

b5=b5+ thdot (t-1) * thdot (t) ; 

b6=b6  +  rudder (t-1) * thdot (t) ; 
end 

eps=le- 6 

%  discrete  heading  parameters. 
thhead=inv( [bl  -b2;-b2  b4] ) * [ -b5 ;b6] ; 
phi= [-thvelo (1)  0 ; 0  -thhead(l)]; 
del= [thvelo(2)  0;  0  thhead(2)]; 
%  discrete  to  continuous  transformation 
[A,B] =d2c(phi,del, 0.022  5)  ; 
prms=[-A(l/l)  ;B(1,1) ;-A(2,2)  ;B(2,2)]; 

end  %  parest 


function  [v, dvx, dvy] =VP512 (x, y , lambda) ; 

% [v,dvx, dvy] =VP (x,y, lambda) 

%  potential  function  for  the  pool. 

%  This  function  calculates  potential  function  at  a  given 

%  point  and  its  derivatives  along  x  and  y 


v0=(x.* (x-12)  ) * (y.* (y-12)  )  '  ; 

z= (vO) / (lambda) ;  v= sigmoid ( z) ; 

dvx=dsig(z) * ( (x- 12 ) * (y . * (y- 12 ) ) ' +x* (y . * (y- 12 ) ) ' ) /lambda; 

dvy=dsig(z) *( (x. * (x-12) ) * (y-12) '+ (x. * (x-12) ) *y' ) /lambda; 


end   %potential 
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function  y=sigmoid(x) 

%  y  =sigmoid(x) 

%  value  of  sigmoid  function 


o  o  o,  o,  <J,  o  o  o  o  ^^^20*3*2r'^^^%*^^2*^^^B*^^2*^2*^^^3*3*2*^2*2*^^2*^^^^2'(}>'Q   9  9  9  9  °  °  °  9  9  ° 
oooooooooooooooooooooooooooooooooooooooooooooooooooooo'^'o'o'o'^"^ 


x=min (x, 100) ;  x=max (x, - 100 )  ; 
y=  (1-exp ( -x) ) . / (1+exp ( -x) ) ; 
end  %  sigmoid 


function  d=dsig(x) 
%derivative  of  sigmoid 
%  d=dsig(x) 


'o'o'o'oo'o'ooo'o'oooooooooooooooooooooooooooo'oooooooooooooooooooooo 


x=min (x, 100) ;x=max (x, -100) ; 

temp=exp ( -x) ; 

d=temp  ./  (1+2* temp  +  temp  . *  temp! 

end 


function  x=showl0 (xm, ym, lambda, LI, L2) 

%plottig  routine  for  displaying  the   estimated  borders  of 

%the  map  and  contours  of  potential  function. 

%  clg;  hold  off; 


x=linspace ( - 1, Ll  +  1) ; 
y=linspace ( - 1 , L2  +  1)  ; 
[v, dvx, dvy] =VP_cyl (x' ,y' , lambda) ; 
contour (v, x, y) 
hold  on 

for  t=l : length (xm) 

plot(xm(t),  ym(t),  ' og' ) , xlabel ( ' X  (in  feet)') 
ylabel ( ' Y ( in  feet) ' ) , 
end 
end  %show 


2'9'*2-9*,9*S-9'2-9-9-9-9-9^9-9-9*9-9-2-^'^S»4-9»0  9  9  °  9  S-9-9-S-0  9  ^-Q-'3'9«■9-2'9«■9«'9'3'9«-9«-9«-5'5'5'9'S'&-9-2-2'9'2-■S,' 
0  o  o  o  o  o  o*o"o*o^*o*o^o  o  o  o^>oo'o'o*o''o^'t>'o^>"o^>,o^>^^>^>^ooooo  00  o  00  o  0000000  o  o  o  00 

function  [v, dvx, dvy] =vp_cyl (x,y, lambda) ; 

% [v, dvx, dvy] =VP_cyl (x, y, lambda) 

%  potential  function  for  the  pool. 

%  this  function  defines  a  cylindrical  buoy  in  the  pool 
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Ll=12 ;L2=12; 

a=9 ;b=9 ; c= . 04 ;   %  radius  of  the  object  is  .2  ft. 

v0= (x.* (x-Ll) . * (x-a) .*2)*(y.*(y-L2))'+(x.*(x-Ll))*(y.*(y-L2) 

.  .  .  * (y-b)  ."2)  ' -c* (x.* (x-Ll)  ) * (y.* (y-L2) )  '  ; 

z= (vO) / (lambda) ;  v=sigmoid (z) ;   v=flipud(v); 

dvx=dsig(z) * ( ( (x-Ll) . * (x-a) . "2 ) * (y . * (y-L2 ) ) ' + (x. * (x-a) . "2 ) 

*(y.*(y-L2))'+(2*x.*(x- LI ).*(x-a))*(y.*(y-L2 ))'... 

+ (x-Ll) * (y.* (y-L2) . * (y-b) . A2) ' +x* (y.*(y-L2).*(y-b)."2)'... 

-c*(x-Ll)*(y.*(y-L2))' -c*x* (y . * (y-L2) ) ' ) /lambda; 

dvy=dsig(z) * ( (x. * (x-Ll) . * (x-a) . A2 ) * (y-L2 ) ' . . . 

+ (x.* (x-Ll) . * (x-a) . A2) *y' + (x.* (x-Ll) ) * ( (y-L2) . * (y-b) ."2) ' . . . 

+(x.*(x-Ll))*(y.*(y-b) . A2) ' + (x. * (x-Ll) ) * (2*y. * (y-L2) . * (y-b) ) 

. . . ' -c* (x.* (x-Ll) ) *(y-L2) ' -c* (x.*(x-Ll) ) *y' ) /lambda; 

end        %vp_cyl 


function  [ zhm] =est_opnp (data , P , zhO , lambda , prms ) 

%  function  zhm=est_opnp (data, P, zhO , lambda, prms) 

%  modified  April  1  st  1994 

%  this  function  commands  estimator  for  the  case  of  open  sea 

OOOOOOOOOOOOOOOOOOOOOOOOOODOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO 

c  1  g ;  c  1  c  ; 

n=length (data) ; 

ns=200; 

for  i=  0:n/ns-l 

[zh, P, zs] =scn_opn (zhO , P, data ( (ns*i+l) : (ns*(i+l) ) , :) , lambda, p 
rms)  ; 

zhm= [zhm,  zh] ; 

zsm= [zsm, zs] ; 

lambda= . 3* lambda 

nz=length ( zh) ;  zh0=zh ( : , nz) ; 

end 

clg 

lambda, pause 

axis ( [0  25  -5  20] ) 

plt_opn2 (25, 15) ; 

hold  on 

plot (zhm(l, : ) ,  zhm (2, : ) , ' *g' , data ( : ,4) , data ( : , 5) ,'-.') , 

title  ('  Estimated  Trajectory  of  AUV'),grid 

xlabelC  X  (in  f  eet )  '  )  ,  ylabel  (  'Y  (in  feet)'), 

gtext (' Estimated  :Solid   line'), 

gtext ( 'Actual     rDashed  line'), 

hold  off 

end     %  est_opnp 


71 


o  o  o  9^S*9^^S*^J^9*!^^J^^9^^9^^9*^^^9*^9*^9^9^J^9*5^  o  o  o  o  o  o 

^^^^^oooooooooooooooooooooooooooooooooooo1i)^ooooooooo^^^*?>'?r',?)*?>'6 

function   [v  , dvx, dvy] =vp_open (x, y, lambda) ; 

%modified  February  19th  1994 

%This  function  calculates  the  potential  function 

%  in  a  environment  without  borders 

%  [v,  dvx,  dv\  .  =vp_open  (x,y,  lambda)  ; 

oooooooooooooooo 

a=5;  b=5;  c=.25; 
v0= (x-a) .A2  + (y-b) .A2-c; 
z=vO/ lambda;  v=sigmoid(z) ; , 
dvx=dsig (z) *2* (x-a) /lambda; 
dvy=dsig (z) *2* (x-b) /lambda; 
end     %  vp_open 


function   [v  ,dvx,  dvy] =vp_open2 (x, y, lambda) ; 

%modified  January  20th  1994. 

%  vp_open2.m 

%function  for  computing  potential  function  for  two  obstacles 

%  in  open  sea. 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

a=10;  b=20;  c= . 25 ;d=16 ; e=10 ; f = . 25 ; 

v0=( (x-a)  .A2)*( (x-d)  .A2)  +  ( (y-b)  . "2) * ( (x-d)  . "2)  -  (c* (x-d)  .  A2)  + 
( (x-a)  ."2)* (y-e)  .A2+( (y-b)  ."2)* (y-e)  .A2-c*(y-e)  ."2-.  .  . 
f*(x-a) .A2-f*(y-b) ."2+c*f; 
z=v0/ lambda;  v=sigmoid(z) ; , 

dvxx=2* (x-a) *( (x-d) .A2)+2*((x-a) . A2) * (x-d) + . . . 
2* (x-d) * ( (y-b) .  A2) -2*c* (x-d) +2* (x-a) * ( (y-e) .  A2) -2*f* (x-a) ; 
dvx=dsig(z) * dvxx/ lambda; 

dvyy=2* (y-b) * ( (x-d) . "2 ) +2* (y-e) * ( (x-a) . "2) +2* (y-b) * . . . 
( (y-e) ."2) +2* (y-e)* ( (y-b) . "2 ) -2*c* (y-e) -2*f * (y-b) ; 
dvy=dsig (z) *dvyy/ lambda  ; 
end  %  vp_open2 


oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 

function   pi t_opn2 (limits , lambda) 
%  routine  for  plotting  potential  function 
%   when  two  obstacles  are  present. 
%modified  January  20th  1994 

oo  ooo'o'o'^'o'o'ooo  o  q'o'v'q'o*o*q1>*q'o^>'q^>'q'q*q*o'oooo*qo   o  o  o  o  o  ooooo  o  oooo  oo  o  o  o  oo  o 

xx=linspace (0, limits) ; , yy=linspace (0 , limits) ; 
[x,  y] =meshdom(xx,yy) ; 
a=10 ; b=2  0 ; c= . 2  5 ; d=16 ; e=10 ; f = . 25 ; 

u= ( (x-a) ."2+ (y-b) .A2-c) .*( (x-d) ."2+ (y-e) ."2-f ) /lambda; 
z= (1-exp ( -u) ) . / (1+exp ( -u) ) ;  ,  %  mesh (z) , pause 
%title (' Potential  Function  When  Two  Obstacles  are  present') 
%meta  alpl 
contour (z, xx, yy) ; 
end   %  plt_opn2 
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.o„o,o,o,o,oi,o,o<,o,o,o,o,o,o,o,o,o,o.o,o,o-,o  o  o  o  o  o  o  o  o  o  o  o  o  o  o 
oooooooooooooooooooooooo*o"o*o*^*^*o*^*^*o"o"o*o 


n  zhm=est_curl (data, P, zhO , lambda, prms) 

%  function  zhm=est_curl (data, P, zhO , lambda, prms) 

%  modified  21  Nov  1993 

%  this  function  commands  the  estimator  for  the  case  of 

^currents 

%  states= [x,  y,  v,  theta, thetadot  , currentx, currenty]  7X1 

vector 

%  P=7X7  error  covariance  vector. 

%  zhO=  initial  state  estimates  7X1  vector. 


clg;  hold  off;clc 
n=length (data) ; 
ns=2  0  0; 

for  i=  0:n/ns-l 

[zh, P, zs] =scn_curl (zhO,P,data( (ns*i+l) : (ns* ( i+1) ) , : ) , lambda , 
prms) 

zhm= [zhm,     zh] ; 

lambda= . 5* lambda 

nz=length (zh) ;  zhO=zh ( : , nz) ; 

end 

clg 

axis ( [0  20  0  25] ) 

plt_opn2 (30,1) 

hold  on 

plot (zhm(l,  : )  ,  zhm  (2,  : )  , ' *g' , data ( : ,4)  , data ( : ,5) , ' - . ' ) 

title  ('  Estimated   Trajectory  of  AUV'),grid 

xlabelCX  (in  f  eet )  '  )  ,  ylabel  ( '  Y  (in  feet)') 

gtext (' Estimated  :Solid   line')  , 

gtext ( 'Actual     : Dashed  line'), 

end     %  est  curl 


OO00O"0*O*0'o'6'*O*0OOO0OOOOOOOOOOOOOOOOOO0OO0OOOOOOOOOOOOOOOOO0OO00 

function  [zh, P, zs] =scn_curl ( zhO , P, data, lambda, prms) 
%  modified  April  2nd  1994 

%  this  function  applies  Kalman  filter  when  there  are 
%  currents  present 
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n=length(data) ; 

zh( : , 1) =zhO; 

R=l;  ^Measurement  error  Covariance. 

al=prms (1) ;bl=prms (2) ;b2=prms (3) ;bl=prms (4) ;Ts=0 .0225 

for  t=l:n-l 

rho=data (t , 2) ; 
alpha=data ( t , 1 ) ; 
headh=zh (4 , t) ; 
RPM(t) =data(t, 6) ; 
RUDDER (t) =data(t,7) 
a=cos (headh*pi/180) 
b=sin(headh*pi/180) 
A=  [  0  0  a  0  0  1  0 ; 
0  0  b  0  0  0  1; 
0  0  -al  0  0  0  0; 
0  0  0  0  10  0; 
0  0  0  0  -bl  0  0; 
0  0  0  0  0  0  0; 
0  0  0  0  0  0  0]; 

B= [ 0  0 ; 0  0 ; a2  0 ; 0  0 ; 0  b2 ; 0  0 ; 0  0 ]  ; 
[Phi, del] =c2d(A,B,Ts) ; 
dx0=rho*cos ( (theta+headh) *pi/l80) ; 
dy0=rho*sin( (theta+headh) *pi/l80) ; 
x0=zh(l, t) +dx0; 
y0=zh(2, t) +dy0; 
zs  (  :  ,  t)  =  [x0;y0]  ; 

[v, dvx, dvy] =vp_open2 (xO , yO , lambda) ; 
h= [dvx, dvy, 0, ( -dvx*dy0+dvy*dx0) *pi/180, 0,0,0]'; 
s=h'*P*h+R;  K=Phi*P*h/s; 
e=0-v; 

zh( : ,t+l) =Phi*zh( : , t) +del* [RPM(t)  RUDDER(t) ] ' +K*e; 
P=Phi*P*Phi' -K*s*K' ; 

end 

end  %  scan  curl 


00000000000000000000000*0^00000000000000000000000000000000000 

function   [zhm, zsmt]  =  est_act2 (data, Pklk, zhO , lambda) 

% [zhm, zsmt] =est_act2 (data, P, zhO , lambda) 

%  Modified  February  3  1994 

%  data  file  must  contain: [bearing (relative) , range] 

%  estimated  states  are  zhm= [x,  y,v,theta] 

%  all  angles  are  in  degrees  ranges  are  in  meters. 

oooooooooo'oo'o'oooooooo'o'o'o'o'o'o'o'o'ooooooooooooooooo'oooooooooooooo 
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!  del  tezgl.met 

clg;  hold  off; 

subplot (221) 

n=length (data) ; 

ns=200; 

for  i=0:n/200-l 

[zh, Pklk, zs,xsmth] =scn_act2 ( zhO , Pklk, data ( (ns*i+l) : (ns* (i+1) 
) , : ) , lambda) ; 

xm=zs(l,:);  ym=zs(2,:); 

zhm= [zhm;  zh] ; 

zsmt= [zsmt, xsmth] ; 

showll (xm,ym, lambda) ;  title ( ' POOL' ) ; 

if  i==3 
meta 

end 

hold  off 

lambda=0 . 5*lambda; 

nz=length ( zh) ;  zhO=zh ( : , nz) ; 

end 

hold  off 

showlO (xm, ym, lambda) ; 

hold  on 

plot (zhm(l,  : )  ,  zhm(2,  : )  , ' *g' )  ,  title  ('  Estimated   Trajectory 

of 

AUV'  ) 

hold  off 

showlO (xm, ym, lambda) ; 

plot ( zsmt ( 1 ,:), zsmt (2 ,:),' *r' ), title ( '  Smoothed  Trajectory 

of  AUV' ) , 

hold  off 

meta 

end  %  est  act2 


function  [zh, Pklk, zs , xsmth] =scn_act2 ( zhO , Pklk, data , lambda) 

%  [zh,  Pklk, zs , xsmth] =scn_act2 ( zhO , Pklk, data, lambda) 

%  Modified  February  3rd  1994 

%  The  estimation  algorithm  for  navigation  of  AUV. 

%  This  function  works  with  actual  data  taken  at  Pool. 

%  Fixed  interval  smoothing  is  applied  to  the  estimated 

%states 

oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo 


n=length (data) 
zh( : , 1) =zhO; 
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dt=0.0225; 

R-l; 

for  t=l:n-l 

headh=zh (4, t) ; 

rho=data ( t ,  2 ) ;  theta=data ( t ,  1 ) ; 
A=[0  0  cos (headh*pi/180)  0; 
0  0  sin(headh*pi/180)  0; 
0  0  0  0; 
0  0  0  0]  ; 
B= [0  0  0  0]  '  ; 
[Phi  del] =c2d(A,B,dt) ; 
dx0=rho*cos ( (theta+headh) *pi/180) ; 
dy0=rho*sin ( (theta+headh) *pi/180) ; 
x0=zh(l; t) +dx0; 
y0=zh(2, t) +dy0; 
zs  (  :  ,  t)  =  [x0;y0]  ; 
[v.dvx, dvy] =VP (x0;y0, lambda) ; 
%Kalman  filter 

H- [dvx,dvy, 0, ( -dvx*dy0+dvy*dx0 ) *pi/l80] ; 
KG=Pklk*H' * (H*Pklk*H#  +R) A ( -1)  ; 
Pkk= (eye (4) -KG*H) *Pklk; 
Pkkc= [Pkkc  Pkk]  ; 
Pklk=Phi*Pkk*Phi' ; 
Pklkc=[Pklkc  Pklk] ; 
zhl ( : , t)=zh( :,t) +KG* (0-v) ; 
zhlc( : , t) =zhl (: ,t) ; 
zh( : , t+1) =Phi*zhl ( : , t) ; 
zhc= [zhc, zh ( : , t+1) ] ; 

end 

%  smooth  data  points 

[xsmth] =smooth ' zhc, zhlc, Pkkc, Pklkc, data) ; 

end  %scn  act 


Q,  0,  Q,  o. 


function  [xsmth] =smooth (zhc, zhlc, Pkkc, Pklkc, data) ; 
%  function  [xsmth] =smooth (zhc, zhlc, Pkkc, Pklkc, data) ; 
%   Modified  February  11th  1994; 

%  This  Subroutine  implements  the  Fixed  interval  smoothing 
%  to  the  previously  stored  error  covariance  and  state 
%estimates . 
%This  function  works  in  reverse  time 

oooooooooooooo'o'oooooooooo^'o''1)''o*^^>^>oooooooooooooooooooooooooooo 

%Flip  the  Previously  stored  Matrices 
Pkkc=fliplr (Pkkc) ; 
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Pklkc=f liplr (Pklkc) ; 

zhlc=f liplr (zhlc) ; 

zhc=f liplr (zhc) ; 

%initial  smoothed  estimate  is  final   estimate  zh; 

XS ( : , 1) =zhc (  :  ,  1) ; 

%initial  smoothed  error  covariance  is  final  forward  time 

%  error  covariance 

Psl=f liplr (Pkkc (: ,1:4) ) ; 

for  i  =  1 : length (data)  -  5 ; 

headh=xs (4 , i) ; 

A=[0  0  cos (headh*pi/180)  0; 

0  0  sin(headh*pi/180)  0; 

0  0  0  0; 

0  0  0  0]  ; 

B=  [0  0  0  0]  '  ; 

[Phi, del] =c2d(A,B,dt) ; 
%  Smoothing  Filter. 

Ak=f liplr (Pkkc ( : , 4*i-3 :4*i) ) *Phi' *. . . 
(f liplr (Pklkc ( :,4*i-3:4*i)))"(-l); 
xs ( : , i+1) =zhlc ( : , i)  +  Ak* (xs ( : , i) - zhc ( : , i) ) ; 
Psl=f liplr (Pkkc ( : ,4*i-3 :4*i) ) +. . . 
Ak* (Psl-f liplr (Pklkc ( : ,4*i-3 :4*i) ) ) *Ak' ; 

end 

%  Flip  the  smoothed  estimates  to  forward  time 
xsmth=f liplr (xs) ; 
end    %  smooth 


%Train2 .m 

%modified  December  17th  1993 

%This  m  file  applies   momentum  with  adaptive  learning  rate 

%algorithm  to  train  input  vectors. 

%  Input  vector  =  [x0,y0]  location  of  the  sonar  tip 

%  Output  target  vector  = [v  dvx  dvy]  potential  function  and 

%its   derivatives 


clc 

%load  nrin2 . dat 
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%load  nrout2.dat 

P=nrin2 
T=nrout2 ; 

%  initialize  network 
[R,Q] =size(P) 
Sl  =  9; 

[S2,Q] =size(T) 
[W1,B1] =nwtan(Sl,R) 
[W2,B2] =rands (S2,S1) 
%Training  parameters 


disp_f req=25 
max_epoch=4  00 ; 
err_goal= . 002 ; 
momentum= . 9  5 ; 
err_ratio=l . 04 ; 
lr=0.5 
lr_inc=l . 05 
lr  dec=.7 


%  display  frequency 
%  maximum  iterations 
%  error  level 
%  amount  of  momentum 
%  error  ratio 
%learning  ratio 
% increment 
%decrement 


%training  parameters 

TP= [disp_f req  max_epoch  err_goal  lr   lr_inc  lr_dec   momentum 

err_ratio] ; 

%  Train  Network 

[Wl , Bl , W2 , B2 , epochs , TR] =trainbpx (Wl , Bl , ' tansig ' , W2  #  B2 , ' purel 

in' , P,T,TP) ; 

end 


ooooooooooooooooooooooooooooooo'ooo'oo'oooooooooo'o'ooooo'oooooo^ 

function  zhm=estneuro (data, P, zhO , prms , Wa, Wb, Ba, Bb) 

%  function  zhm=estneuro (data, P, zhO , prms ,Wa,Wb; Ba, Bb) 

%  modified  11  Nov  1993 

%this  function  implements  neural  networks  for  estimation. 
Wa,Ba  are  weights  and  biases  for  first  (sigmoid)  layer 
Wb,Bb  are  weights  and  biases  for  second  (linear)  layer 


ooooooooooooooooooooooooooooooooo"o*o*o*ooooooooooooooooooooooo 


clg;  hold  off;clc 
subplot (221) 
n=length (data) ; 
Ts=0.0225; 
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[zh, P, zs] =scnneuro (zhO , P, data, prms , Wa, Wb, Ba, Bb) ; 

axis ( [0  20   0  25] ) 

plt_opn2 (25 , . 1  ) , hold  on, 

plot (zhm(l, : ) ,  zhm(2, : ) , ' *g' , data ( : ,4) , data ( : , 5) , ' - . ' ] 

title('  Estimated   Trajectory  of  AUV  using  BNN'),grid 

xlabel ( '  X(in  f eet ) ' ) , ylabel ( '  Y(in  feet)'), 

gtext (' Estimated  :Solid   line')  , 

gtext (' Actual     :Dashed  line'), 

end     %  estneuro 


function  [zh, P, zs] =scnneuro (zhO , P, data, prms , Wa, Wb, Ba, Bb) 

%modified   November  11th  1993 

%  this  function  applies  the  estimation  algorithm  using 

%  back  propagation  neural  networks 

%potential  function  is  replaced  by  neural  network. 

%  Wa,Ba  are  weight  and  biases  for  first  (sigmoid  )  layer 

%  Wb,  Bb  are  weight  and  biases  for  second  (linear)  layer 

0000000000000000000000*00000000000000000000000000000000000000 

n=length (data) ; 

zh ( : , 1) =zhO; 

R=l;  %Measurement  error  Covariance. 

al=prms (1)  ;a2=prms  (2)  ;b2=prms (3)  ;bl=prms (4)  ;Ts  =  0 .022  5; 

for  t=l:n-l 

rho=data (t , 2) ; 
alpha=data ( t , 1) ; 
headh=zh (4 , t ) ; 
RPM(t) =data(t, 6) ; 
RUDDER (t) =data(t,7) 
a=cos (headh*pi/180) 
b=sin (headh*pi/l80) 
A= [0  0  a  0  0 
0  0  b  0  0 
0  0  -al  0  0 
0  0  0  0  1 
0  0  0  0  -bl]  ; 

B= [ 0  0 ; 0  0 ; a2  0 ; 0  0 ; 0  b2  ]  ; 
[Phi, del] =c2d(A,B,Ts) ; 
dxO=rho*cos ( (alpha+headh) *pi/180) ; 
dyO=rho*sin ( (alpha+headh) *pi/180) ; 
xO=zh(l, t) +dxO; 
y0=zh(2, t) +dyO; 
zs  (  :  ,  t)  =  [xO;yO]  ; 

%  implementation  of  neural  network. 
Aa=tansig ( (Wa* [xO ;yO] ) , Ba) ;       %  sigmoid  neuron  layer 
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Ab=purelin (Wb*Aa; Bb) ;  %  linear  neuro   layer 

v=Ab ( 1 ) ; dvx=Ab ( 2 ) ; dvy=Ab ( 3 ) ;      %  network  output 

h= [dvx;dvy, 0, ( -dvx*dyO+dvy*dxO ) *pi/180, 0] ' ; 

s=h'*P*h+l;  K=Phi*P*h/s; 

e=0-v; 

zh( : , t+1) =Phi*zh( : , t) +del* [RPM(t)  RUDDER(t) ] ' +K*e; 

P=Phi*P*Phi' -K*s*K' ; 

end 

end  %scnneuro 


%PRDATA6.M 

%  modified  November  18  1993 

%this  program  simulates  the  motion  of  AUV  in  the  rectangular 

%pool 

%  several  obstacles  are  added  in  the  environment. 

"5  o  o^^^^^^^^^^^^'6'6'o'6'6'S'o'o'5''5'S'5"5'o'o'o'6'6'o'S'8'S'5''o'5'o'5'o"o'o'5'S''5  o  o  o  o  o  o  o  o  o  o  o  o 


clear, clg, clc 

L1=12;L2=12; 

Ts=0.0225; 

Tf=7*9; 

kmax=Tf /Ts+1; 

x= zeros (5 , kmax) ; 

x(:,l)  =  [2  4  3  0  0]  '  ; 

t  ime ( 1 ) =  0 ; 

RPM=150*ones (l,kmax) ; 

RUDDER=zeros ( 1 , kmax) ; 

%dynamic  parameters 

al-1; 

a2=0. 001; 

b2  =  l; 

bl=l; 

for  k=l:kmax-l 

time (k+l) =time (k) +Ts ; 
tetha=x(4 , k) ; 
a=cos (tetha*pi/180) ; 
b=sin(tetha*pi/180) ; 
A= [ 0  0  a  0  0 

0  0  b  0  0 

0  0  -al  0  0 

0  0  0  0  1 

0  0  0  0  -bl] ; 
B= [ 0  0 ; 0  0 ; a2  0 ; 0  0 ; 0  b2  ]  ; 
E=eye(5) ; 
[phi, dell] =c2d(A,B,Ts) ; 
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[phi, del2] =c2d(A,E,Ts) ; 
if  time (k) >=10 

RUDDER (k) =3; 
end 
if  time (k) >=35 

RUDDER ( k ) =  0  ; 
end 

rand ( ' normal ' ) 

ex=0. 01 * rand; 

ey=0 . 01*rand; 

ev=0 . 01*rand; 

et=0 .  01*rand; 

etd=0. 01*rand; 

x(  : ,k+l) =phi*x( : ,k) +dell* [RPM(k)  RUDDER (k) ]' +del2* [ex  ey 
ev 
et  etd] ' ; 

head(k) =x(4,k) ; 

shdg(k) =rem(head(k) +0.9*k,3  60) ; 

shdgl (k) =rem(0.9*k, 360) ; 

if  shdg(k)  >  180 

shdg(k) =-360+shdg(k) ; 

end 

if  shdgl (k)  >  180 

shdgl (k) =-360+shdgl (k) +0.01*rand; 

end 

angl (k) =180/pi*atan2 ( (3-x(2,k) ) , (9-x(l,k) ) ) ;   %  obstacle 
at  (9,3) 

ang2 (k) =180/pi*atan2 ( (9-x(2,k) ) , (3-x(l,k) ) ) ;   %  obstacle 
at  (3,9) 

[r,h] =rangeh(x(l,k) ,x(2,k) , shdg(k) ,  LI, L2 ) ; 

if  (shdg(k) >=angl (k) -3) &(shdg(k) <=angl (k) +3) ; 

dist (k) =sqrt ((9-x(l,k))A2+(3-x(2,k))"2) +0.01*rand; 
elseif  (shdg(k) >=ang2 (k) -2) &(shdg(k) <=ang2 (k) +2) ; 

dist (k) =sqrt ((3-x(l,k))x2+(9-x(2,k))"2) +0.01*rand; 
else 

dist (k) =r+0 . 01*rand; 
end 
end 

temp(k, : )= [shdgl (k)  dist(k)  x(3,k)  x(l,k)  x(2,k)  RPM(k) 
RUDDER (k) . . . 

x(4,k)  x(5,k) ] ; 
end 

!del  datalp5.dat 
%  save  data  in  ascii  code 
save  datalp5.dat  temp  /ascii 
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ooooooooooooooooooooooooooooooooooooooooo"o*o"ooooooo'o'o*o*o'o"o'o*o'o'$ 

function  [r, h] =rangeh (x,y, theta, Ll, L2) 

%  [r,h] =range (x,y, theta, Ll, L2 ) 

%  this  function  computes  sonar  range  rho 

%  and  its  gradient  h 

%  at  position  (x,y)  with  heading  theta 

%  in  the  Ll  by  L2  pool . 

%  h= [dr/dtheta,  dr/dx,  dr/dy] 

oooooo"booooooooooooooooooooooooooooooooooooooooooooooooooooo 

theta=theta*pi/180 ; 
thl=atan2 ( -y, -x) ; 
th2=atan2 (L2-y, -x) ; 
th3=atan2 (L2-y, Ll-x) ; 
th4=atan2 ( -y, Ll-x) ; 
while  (theta  >thl+2*pi) , 

theta=theta-2*pi ; 
end 
while  (theta  <=th4), 

theta=theta+2*pi ; 
end 

c=cos (theta) ; s=sin (theta) ; 
if  (theta>=th2) &(theta<thl+2*pi) , 

r=-x/c; 

h= [-x*s/c"2, -l/c,0] ; 
end 
if  (theta>=th3) &(theta<th2) , 

r=(L2-y)/s; 

h=[(y-L2)*c/s"2,0, -1/s] ; 
end 
if  (theta>=th4) &(theta<th3) , 

r= (Ll-x) /c; 

h= [ (Ll-x) *s/cA2, -l/c,0] ; 
end 
if  (theta>=thl+2*pi) & ( theta<th4+2*pi) , 

r=-y/s; 

h=[-y*c/sA2,0, -1/s] ; 
end 
end  % range 


ooooooo'oo'ooooooooooooooo'o'o'o'o'oooooooooooooooooooooooooooooooo 

%pr_open.m 

%This  program  simulates  the  motion  of  AUV 

%in  a  environment  without  borders 

%modified  25  oct  93 

Iseveral  obstacles  in  the  environment  can  be  added. 

o  o  000000000*00  00  o  00  o  o  o  o'b'o'o'o'o'o'o'o'o'oo'o'o'o'o'oooo  o  o  o  o  000  o  o  o  o  o  o  o  o  o  o  00 


clear, clg, clc 
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Ts=0. 0225; 

Tf=7*9; 

kmax=Tf /Ts+1; 

x=zeros ( 5 , kmax) ; 

x(:  ,1)  =  [3  3  3  45  0  ]  '  ; 

time (1) =0; 

RPM=150*ones (l,kmax) ; 

RUDDER= zeros ( 1 , kmax) ; 

al  =  l; 

a2=0.001; 

bl-1; 

b2=l; 

for  k=l:kmax-l 

t  ime ( k+ 1 ) = t ime ( k ) +Ts ; 
tetha=x (4 , k) ; 
a=cos (tetha*pi/180) ; 
b=sin (tetha*pi/180) ; 
A= [0  0  a  0  0 


0    0    b    0 

0 

0    0    -al 

0    0 

0    0    0    0 

1 

0    0    0    0 

-bl]  ; 

B=[0    0;0    0, 

; a2    0 ; 0    0 ; 0    b2 ] ; 

E=eye(5) ; 

[phi, dell] « 

=c2d(A,B,Ts) ; 

[phi,del2] = 

=c2d(A, E,Ts) ; 

if  time (k) >=3  0 ;         ; 

RUDDER (k) =10; 
end 
if  time  (k)  >=33 

RUDDER (k) =0; 
end 

rand ( ' normal ' ) 

W=0 . 01*rand* [11111]' 

x(: ,k+l) =phi*x( : ,k) +dell* [RPM(k)  RUDDER (k) ] ' +del2*W; 
head(k) =x(4,k) ; 

shdg(k) =rem(head(k) +0.9*k,360) ; 
shdgl (k) =rem(0.9*k,360) ; 
if  shdg(k)  >  180 

shdg (k) =-360+shdg (k) ; 
end 
if  shdgl (k)  >  180 

shdgl (k) =-360+shdgl (k) +0.01*rand; 
end 

angl  (k)  =18  0/pi*atan2  (  (10-x  (2  ,  k)  )  ,  (5-x(l,Jc)  )  )  ;        %lst 
obstacle  at  (5, 10) 

ang2 (k) =180/pi*atan2 ( (5 -x (2 , k) ) , ( 10 -x ( 1 , k) ) ) ; %    2nd 
obstacle  at  (10,5) 
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ang3 (k) =180/pi*atan2 ( (5-x(2,k) ) , (18-x(l,k) ) ) ;  %  3rd 
obstacle  at  (18,5) 

ang4 (k) =180/pi*atan2 ( (10-x (2 , k) ) , (2 -x (1, k) ) ) ; %  4th 
obstacle  at  (2, 10) 

ang5 (k) =180/pi*atan2 ( (20-x (2 , k) ) , (8-x (1, k) ) ) ; %  5th 
obstacle  at  (8,20 


if  (shdg (k) >=angl (k) -2) &(shdg (k) <=angl (k) +2) ; 

dist (k)=sqrt ( (5-x(l , k) ) "2+ (10-x (2 , k) ) "2 ) +0.01*rand; 
elseif  (shdg(k) >=ang2 (k) -2) &(shdg (k) <=ang2 (k) +2) ; 

dist (k)=sqrt ( (10-x (1, k) ) A2+ (5-x (2 , k) ) A2) +0.01*rand; 
elseif  (shdg(k) >=ang3 (k) -2) &(shdg(k) <=ang3 (k) +2) ; 

dist (k)=sqrt ( ( 18 -x (1 , k) ) A2+ (5 -x (2 , k) ) A2 ) +0.01*rand; 
elseif  (shdg (k) >=ang4 (k) -2) &(shdg (k) <=ang4 (k) +2) ; 

dist (k) =sqrt ( (2 -x (1, k) ) "2+ (10-x (2 , k) ) "2 ) +0.01*rand; 
elseif  (shdg (k) >=ang5 (k) -2) &(shdg (k) <=ang5 (k) +2) ; 

dist (k) =sqrt ((8-x(l,k))"2+(20-x(2,k))"2) +0.01*rand; 
else 

dist (k) =500+0 . 01*rand;  %  add  very  large  number 
end 
temp(k, : ) = [shdgl (k)  dist (k)  x(3,k)  x(l,k)  x(2,k)  RPM(k: 
RUDDER(k)  x(4,k)  x(5,k)]; 
end 

del  datalpl.dat 

%  save  data  in  ascii  format  in  a  data  file 
save  datfebl.dat  temp  /ascii 
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